#include <math.h>
#include <Servo.h>
#include <TinyGPS.h>
#include <Wire.h>

TinyGPS gps;            //gps
Servo myservo, mysteer;  //motor,kormány
int frontDist = 2;      //érzékelők
int leftDist = 1;
int rightDist = 3;

//COMP
int compAddress=0x42;
int compSlaveAddress, compValue;
byte compData[2];
float robotDeg;

float lat, lon, distance, dlat, dlon;
int course;
unsigned long age;

const int destCount = 7;
int destIndex = 0;
float dlatArray[destCount] = {
47.4715003967,
47.4714088439,
47.4715499877,
47.4717102050,
47.4716796875,
47.4718589782,
47.4717292785
};
float dlonArray[destCount] = {
19.0623893737,
19.0619907379,
19.0613803863,
19.0613708496,
19.0618896484,
19.0632190704,
19.0632991790
};

bool end = false;

void setup() 
{ 
  myservo.attach(9);
  myservo.write(93);
  
  mysteer.attach(8);
  mysteer.write(90);
  
  //COMP
  compSlaveAddress=compAddress>>1;
  Wire.begin();
  
  Serial.begin(115200); // usb monitor
  Serial1.begin(9600); // GPS
  
  dlat = dlatArray[destIndex];
  dlon = dlonArray[destIndex];
  
  Wire.begin();
}

void loop() 
{
  while (!end && Serial1.available())
  {
    getInfo();
    setDirection();
    setSpeed();
  }
}

void getInfo()
{
  if(feedgps())
  {
    compGetData();
    gps.f_get_position(&lat, &lon, &age);
    course = gps.course_to(lat, lon, dlat, dlon) - robotDeg;
    if (course < 0)
    {
      course += 360;
    }
    distance = gps.distance_between(lat, lon, dlat, dlon);
    Serial.print("Lat: ");
    Serial.print(lat, 10);
    Serial.print("  Lon: ");
    Serial.print(lon, 10);
    Serial.print("  Age: ");
    Serial.println(age);
    Serial.print("Course: ");
    Serial.println(gps.course_to(lat, lon, dlat, dlon));
    Serial.print("Compass: ");
    Serial.println(robotDeg);
    Serial.print("Distance: ");
    Serial.println(distance);
    Serial.print("dlat: ");
    Serial.println(dlat, 10);
    Serial.print("dlon: ");
    Serial.println(dlon, 10);
    Serial.print("destIndex: ");
    Serial.println(destIndex);
    Serial.print("end: ");
    Serial.println(end);
    /*Serial.print("Front distance: ");
    Serial.println(analogRead(frontDist));
    Serial.print("Left distance: ");
    Serial.println(analogRead(leftDist));
    Serial.print("Right distance: ");
    Serial.println(analogRead(rightDist));*/
  }
}

bool feedgps()
{
  bool ret = false;
  for (unsigned long start = millis(); millis() - start < 1000;)
  {
    while (Serial1.available())
    {
      char betu = Serial1.read();
      if (gps.encode(betu))
      {
        ret = true;
      }
    }
  }
  return ret;
}

void setDirection()
{
  //delay(1000);
  if (course == 0)
  {
    mysteer.write(93);
  }
  else if (course > 0 && course < 90)
  {
    mysteer.write(110);
  }
  else if (course >= 90 && course < 180)
  {
    mysteer.write(130);
  }
  else if (course >= 180 && course < 270)
  {
    mysteer.write(50);
  }
  else
  {
    mysteer.write(70);
  }
}

void setSpeed()
{
  if (distance > 40)
  {
    myservo.write(150);
  }
  else if (distance > 7)
  {
    myservo.write(130);
  }
  else
  {
    myservo.write(90);
    destIndex = (destIndex + 1) % destCount;
    dlat = dlatArray[destIndex];
    dlon = dlonArray[destIndex];
    delay(2000);
    
    // Visszajutott a starthoz:
    if (destIndex == 0)
    {
      end = true;
      myservo.write(90);
      mysteer.write(93);
    }
  }
}

void compGetData()
{
	Wire.beginTransmission(compSlaveAddress);
	Wire.write("A");
	Wire.endTransmission();
	delay(10);

	Wire.requestFrom(compSlaveAddress,2);
	int c=0;
	while(Wire.available()&&c<2)
	{
		compData[c]=Wire.read();
		c++;
	}
	compValue=compData[0]*256.0+compData[1];

	//Serial.println("----COMPASS----");
	//Serial.print("Degree: ");
	robotDeg=compValue/10.0+(0.1*(compValue%10));
	//Serial.println(robotDeg);
}
